package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.templates.HardwareMap;
import edu.wpi.first.wpilibj.DriverStationLCD;

public class DoDriveTurn extends CommandBase {
    //Same as in DoDriveDistance
    double Speed;
    double Angle;

    public DoDriveTurn(double s, double a) {
        requires(MyDriveTrain);
        this.Speed=s;
        this.Angle=a;
    }

    protected void initialize() {
        MyDriveTrain.ResetEncoders();
        MyDriveTrain.StartEncoders();
    }

    protected void execute() {
        //Same as in DoDriveDistance
        MyDriveTrain.TankDrive(Speed,-Speed);
        DriverStationLCD lcd = DriverStationLCD.getInstance();
        DriverStationLCD.getInstance().println(
                DriverStationLCD.Line.kUser2, 1,
                "LEncoder: " + MyDriveTrain.GetLeftEncoderValue());
        DriverStationLCD.getInstance().println(
                DriverStationLCD.Line.kUser3, 1,
                "REncoder: " + MyDriveTrain.GetRightEncoderValue());
        lcd.updateLCD();
    }

    protected boolean isFinished() {
        //This isn't correct to determine when the command finishes; as far as I 
        //know, you need to print out the encoder values to the dashboard when 
        //making a turn in teleop and divide that by the angle of the turn  
        if(Angle-(MyDriveTrain.GetLeftEncoderValue()+MyDriveTrain.GetRightEncoderValue())/2 <= 0){
            return(true);
        }
        else{
            return(false);
        }
    }

    protected void end() {
        MyDriveTrain.StopEncoders();
    }

    protected void interrupted() {
    }
}